#!/usr/bin/env python
import json
import os
import rostest
import sys
import threading
import unittest

import rospy
from std_msgs.msg import String
from std_msgs.msg import Empty
from std_msgs.msg import Bool
from platform_controller_msgs.msg import Patients
from platform_controller_msgs.msg import Patient
from platform_controller_msgs.msg import PatrolTask

from autobahn.twisted.websocket import WebSocketServerProtocol

from twisted.internet import reactor
from autobahn.twisted.websocket import (WebSocketClientProtocol,
                                        WebSocketClientFactory)

from autobahn.twisted.websocket import WebSocketServerFactory, listenWS
from autobahn.websocket.compress import (PerMessageDeflateOffer,
                                         PerMessageDeflateOfferAccept)

from twisted.python import log
log.startLogging(sys.stderr)

def shutdown_hook():
    try:
        reactor.stop()
    except ReactorNotRunning:
        rospy.logwarn("Can't stop the reactor, it wasn't running")

def dict_array_sort(dict_array, key, method):
    if(method == 'acsend'):
        for i in range(len(dict_array)-1):
            for j in range(len(dict_array)-1-i):
                if(float(dict_array[j][key]) > float(dict_array[j+1][key])):
                    dict_array[j],dict_array[j+1] = dict_array[j+1],dict_array[j]
    elif(method == 'desend'):
        for i in range(len(dict_array)-1):
            for j in range(len(dict_array)-1-i):
                if(float(dict_array[j][key]) < float(dict_array[j+1][key])):
                    dict_array[j],dict_array[j+1] = dict_array[j+1],dict_array[j]
    else:
        exit('unkown method, you could specify as acsend of descend')
    
    return dict_array

class PatrolProtocol(WebSocketServerProtocol):
    def __init__(self):
        # pub the rosbag_record_start msgs
        self._rosbag_record_start_pub = rospy.Publisher(
          '/launch_rosbag_record', String, queue_size=10, latch=True)
        # pub the rosbag_record_end msgs
        self._rosbag_record_end_pub = rospy.Publisher(
          '/close_rosbag_record', Empty, queue_size=10, latch=True)
        # pub the add_patrol_point msgs
        self._add_patrol_point_pub = rospy.Publisher(
          '/add_patrol_point', String, queue_size=10, latch=True)
        # pub the save_patrol_point msgs
        self._save_patrol_point_pub = rospy.Publisher(
          '/save_patrol_point', String, queue_size=10, latch=True)
        # pub the loadPoints msgs
        self._loadPoints_pub = rospy.Publisher(
          '/loadPoints', String, queue_size=10, latch=True)

        # pub the loadPatients msgs
        self._loadPatients_pub = rospy.Publisher(
          '/load_patients', String, queue_size=10, latch=True)

        # pub the launch_amcl msgs
        self._launch_amcl_pub = rospy.Publisher(
          '/launch_amcl', String, queue_size=10, latch=True)
        # pub the PatrolTask msgs
        self._patrol_task_pub = rospy.Publisher(
          '/patrol_task', PatrolTask, queue_size=10, latch=True)
        # pub the isEnablePatrol msgs
        self._isEnablePatrol_pub = rospy.Publisher(
          '/isEnablePatrol', Bool, queue_size=10, latch=True)
        # pub the close_amcl msgs
        self._close_amcl_pub = rospy.Publisher(
          '/close_amcl', Empty, queue_size=10, latch=True)

        # define the subscriber for patients
        self._patients_sub = rospy.Subscriber("/patients", Patients, self.patients_callback)

        # define the subscriber for operation results
        self._operation_results_sub = rospy.Subscriber("/operation_result", String, self.operation_result_callback)

    def patients_callback(self, msg):
        # rospy.loginfo(rospy.get_caller_id() + "I heard %s", msg.data)
        # rospy.loginfo(rospy.get_caller_id() + "I heard %s")
        patients_dict_array = []
        for patient in msg.patients:
            # rospy.loginfo("[name]:%s, [sex]:%s, [room_num]:%d, [id]:%d", patient.name, patient.sex, patient.room_num, patient.id)
            patients_dict = {'name':patient.name, 'sex':patient.sex, 'room_num':patient.room_num, 'id':patient.id}
            patients_dict_array.append(patients_dict)

        # sort the dict array by the room_num
        dict_array_sort(patients_dict_array, 'room_num', 'acsend')

        _sendDict = {
            'op':'patients_ack',
            'data': json.dumps(patients_dict_array).encode('utf-8')
        }
        
        self.sendMessage( json.dumps(_sendDict).encode('utf-8') )
        
    def operation_result_callback(self, msg):
        message_dict = json.loads(msg.data)
        # print(message_dict['operation_type'])
        # print(message_dict['result'])
        if(message_dict['operation_type'] == 1):
            if(message_dict['result'] == 1):
                _sendDict = {
                    'op':'load_points_ack',
                    'data': 'success'
                }
                self.sendMessage( json.dumps(_sendDict).encode('utf-8') )
            


    def onOpen(self):
        self._sendDict({
            'op': 'shakehands_ack',
            'data': 'none',
        })

    def _sendDict(self, msg_dict, times=1):
        msg = json.dumps(msg_dict).encode('utf-8')
        for _ in range(times):
            self.sendMessage(msg)

    def onMessage(self, message, binary):
        rospy.loginfo('received message {}'.format(message))
        message_dict = json.loads(message)
        if(message_dict['op'] == 'publish'):
            if(message_dict['topic'] == '/launch_rosbag_record'):
                msg = String()
                msg.data = 'success'
                self._rosbag_record_start_pub.publish(msg)
            elif(message_dict['topic'] == '/add_patrol_point'):
                msg = String()
                msg.data = message_dict['data']
                self._add_patrol_point_pub.publish(msg)
            elif(message_dict['topic'] == '/close_rosbag_record'):
                msg = Empty()
                self._rosbag_record_end_pub.publish(msg)
                # save patrol point
                msg = String()
                msg.data = 'success.json'
                self._save_patrol_point_pub.publish(msg)
            elif(message_dict['topic'] == '/launch_navigation'):   

                msg = String()
                msg.data = 'patrol_map_yaml.yaml'
                self._launch_amcl_pub.publish(msg)

                rospy.sleep(3)

                msg = String()
                msg.data = 'patrol_points.json'
                self._loadPoints_pub.publish(msg)
                
                rospy.sleep(1)

                msg = String()
                msg.data = 'patient.csv'
                self._loadPatients_pub.publish(msg)
            elif(message_dict['topic'] == '/patrol_task'):
                msg = PatrolTask()
                msg.type = 0
                msg.time = 15
                msg.mode = 2
                # print(message_dict['index'])
                msg.index = message_dict['index']
                self._patrol_task_pub.publish(msg)
            elif(message_dict['topic'] == '/isEnablePatrol'):                
                msg = Bool()
                msg.data = False
                self._isEnablePatrol_pub.publish(msg)
            elif(message_dict['topic'] == '/close_amcl'):                
                msg = Empty()
                self._close_amcl_pub.publish(msg)
            elif(message_dict['topic'] == '/load_patients'):                
                msg = String()
                msg.data = 'patient.csv'
                self._loadPatients_pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node("patrol_rosbridge_websocket")

    ##################################################
    # Parameter handling                             #
    ##################################################
    retry_startup_delay = rospy.get_param('~retry_startup_delay', 2.0)  # seconds
    if "--retry_startup_delay" in sys.argv:
        idx = sys.argv.index("--retry_startup_delay") + 1
        if idx < len(sys.argv):
            retry_startup_delay = int(sys.argv[idx])
        else:
            print("--retry_startup_delay argument provided without a value.")
            sys.exit(-1)

    use_compression = rospy.get_param('~use_compression', False)

    def handle_compression_offers(offers):
        if not use_compression:
            return
        for offer in offers:
            if isinstance(offer, PerMessageDeflateOffer):
                return PerMessageDeflateOfferAccept(offer)

    protocol = "ws"
    port = rospy.get_param('~port', 9090)
    address = rospy.get_param('~address', "0.0.0.0")
    url = '{}://{}:{}'.format(protocol, address, port)

    print(url)

    factory = WebSocketServerFactory(url)
    factory.protocol = PatrolProtocol
    factory.setProtocolOptions(
        perMessageCompressionAccept=handle_compression_offers,
        autoPingInterval=0,
        autoPingTimeout=30,
    )

    connected = False
    while not connected and not rospy.is_shutdown():
        try:
            listenWS(factory, None)
            rospy.loginfo('Rosbridge WebSocket server started at {}'.format(url))
            connected = True
        except CannotListenError as e:
            rospy.logwarn("Unable to start server: " + str(e) +
                          " Retrying in " + str(retry_startup_delay) + "s.")
            rospy.sleep(retry_startup_delay)

    rospy.on_shutdown(shutdown_hook)
    reactor.run()
